
#include "pid.h"

void PID_Init(positionpid *PID,float Kp,float Ki,float Kd,float T,float MAX,float MIN) //TÎª²ÉÑùÆµÂÊ£¬MAXÎªÊä³öÉÏÏÞ£¬MINÎªÊä³öÏÂÏÞ
{
	PID->Error[0] = 0;
	PID->Error[1] = 0;
	PID->E_sum = 0;
	PID->Kp = Kp;
	PID->Ki = Ki;
	PID->Kd = Kd;
	PID->MAX = MAX;
	PID->MIN = MIN;
	PID->delta_t = 1.0f/(float)T;
}

float PID(positionpid *PID,float SET,float Actual) 
 
{		  
	float pid_out; 
 
	PID->Error[0] = SET-Actual;               	
	
	if(PID->Error[0]>-0.015f&&PID->Error[0]<0.015f)	  
	{
			 PID->Error[0]=0;
		   PID->Error[1]=0;
	}	
	PID->E_sum +=  PID->Error[0]*PID->delta_t;                        		
  if(PID->E_sum>200000)PID->E_sum=200000;          
	if(PID->E_sum<-200000)PID->E_sum=-200000;	
	
	pid_out= PID->Kp * PID->Error[0] + PID->Ki * PID->E_sum +PID->Kd * (PID->Error[0]-PID->Error[1]);           		
	PID->Error[1]=PID->Error[0];
		
	if(pid_out > PID->MAX)pid_out = PID->MAX;             
	if(pid_out <=  PID->MIN)pid_out = PID->MIN;		
     return pid_out;					
}